%% ����Ŀ�꺯��
function cost=CostFunction(sol,model)
    
%     J_inf = inf;
    J_inf= 10e+10;
    n = model.n;
    H = model.H;
    
    % ����
    sol = Transforms(sol,model);
    
    x=sol(1,1:model.n);
    y=sol(1,model.n+1:2*model.n);
    z=sol(1,2*model.n+1:end);

    % ���
    xs=model.start(1);
    ys=model.start(2);
    zs=model.start(3);
    
    % �յ�
    xf=model.end(1);
    yf=model.end(2);
    zf=model.end(3);
    
    x_all = [xs x xf];
    y_all = [ys y yf];
    z_all = [zs z zf];
    
    N = size(x_all,2); % ·������
    
    % ��ƽ��߶� = z_relative + ground_level
    z_abs = zeros(1,N);
    for i = 1:N
        z_abs(i) = z_all(i) + H(ceil(y_all(i)),ceil(x_all(i)));
    end
    
    %============================================
    % F1 - ·�����ȴ��� 
    F1 = 0;
    for i = 1:N-1
        diff = [x_all(i+1) - x_all(i);y_all(i+1) - y_all(i);z_abs(i+1) - z_abs(i)];
        F1 = F1 + norm(diff);
    end

    %==============================================
    % F2 -    �ϰ��ɱ�
    % �ϰ�
    threats = model.threats;
    threat_num = size(threats,1);
    
    drone_size = 1;
    danger_dist = 10*drone_size;
    
    F2 = 0;
    for i = 1:threat_num
        threat = threats(i,:);
        threat_x = threat(1);
        threat_y = threat(2);
        threat_radius = threat(4);
        for j = 1:N-1
            % ͶӰ�߶����ϰ�ԭ��֮��ľ���
            dist = DistP2S([threat_x threat_y],[x_all(j) y_all(j)],[x_all(j+1) y_all(j+1)]);
            if dist > (threat_radius + drone_size + danger_dist) % ����ײ
                threat_cost = 0;%��в���ۼ���
            elseif dist < (threat_radius + drone_size)  % ��ײ
                threat_cost = J_inf;
            else  % Σ��
                threat_cost = (threat_radius + drone_size + danger_dist) - dist;
            end
            F2 = F2 + threat_cost;
        end
    end

    %==============================================
    % F3 - �߶ȳɱ�
    %  ����������У�z, zmin��zmax������ڵ���ĸ߶�
    zmax = model.zmax;
    zmin = model.zmin;
    F3 = 0;
    for i=1:n        
        if z(i) <= 0   % ײ�����
            F3_node = J_inf;
        else
            F3_node = abs(z(i) - (zmax + zmin)/2); %�߶ȳɱ����ۼ���
        end
        
        F3 = F3 + F3_node;
    end
    
    %==============================================
    % F4 - Smooth �ɱ�
    F4 = 0;
    turning_max = 45;
    climb_max = 45;
    for i = 1:N-2
        
        % ͶӰ
        for j = i:-1:1
             segment1_proj = [x_all(j+1); y_all(j+1); 0] - [x_all(j); y_all(j); 0];
             if nnz(segment1_proj) ~= 0
                 break;
             end
        end

        for j = i:N-2
            segment2_proj = [x_all(j+2); y_all(j+2); 0] - [x_all(j+1); y_all(j+1); 0];
             if nnz(segment2_proj) ~= 0
                 break;
             end
        end
     
        climb_angle1 = atan2d(z_abs(i+1) - z_abs(i),norm(segment1_proj));
        climb_angle2 = atan2d(z_abs(i+2) - z_abs(i+1),norm(segment2_proj));
       
        turning_angle = atan2d(norm(cross(segment1_proj,segment2_proj)),dot(segment1_proj,segment2_proj));
       
        if abs(turning_angle) > turning_max
            F4 = F4 + abs(turning_angle);
        end
        if abs(climb_angle2 - climb_angle1) > climb_max
            F4 = F4 + abs(climb_angle2 - climb_angle1);
        end
       
    end

    %============================================
    % Ȩ��ϵ��
    b1 = 8;
    b2 = 9;
    b3 = 10;
    b4 = 1;
    % �ܳɱ�
    cost = b1*F1 + b2*F2 + b3*F3 + b4*F4;%�ܳɱ�����
end